/*
 * mobile.c
 *
 *  Created on: Dec 4, 2023
 *      Author: boen
 */
#include <mobile.h>

#if MOTOR_DC520
static uint32_t mobile_tick =0;															// 分频计数器
static motor_cfg_t motor_param[PHY_MOTOR_MAX];											// 电机参数
static uint8_t mobile_run_axis = MOBILE_AXIS_NULL;										// 底盘运动轴向(X,Y)
static int8_t mobile_run_dir = 0;														// 底盘运动方向(+,-)
static float mobile_run_speed = APP_F_NULL;												// 底盘运动速度(m/s)
static float mobile_run_angle = APP_F_NULL;												// 底盘旋转角度(deg)
static pose_t mobile_target;															// 底盘运动目标点

/**
* @brief 电机初始化函数
* @param 无
* @retval 无
*/
static inline void motor_init(void)
{
    pid_init(&motor_param[PHY_MOTOR_1].PID, MOTOR_1_KP, MOTOR_1_KI, APP_F_NULL);
    pid_init(&motor_param[PHY_MOTOR_2].PID, MOTOR_2_KP, MOTOR_2_KI, APP_F_NULL);
    pid_init(&motor_param[PHY_MOTOR_3].PID, MOTOR_3_KP, MOTOR_3_KI, APP_F_NULL);
    pid_init(&motor_param[PHY_MOTOR_4].PID, MOTOR_4_KP, MOTOR_4_KI, APP_F_NULL);

    //电机1
    motor_param[PHY_MOTOR_1].pEncoderTim = &MOTOR_1_FB;
    motor_param[PHY_MOTOR_1].pPwmTimA = &MOTOR_1_PWMA;
    motor_param[PHY_MOTOR_1].pPwmTimB = &MOTOR_1_PWMB;
    motor_param[PHY_MOTOR_1].TIM_ChannelA = MOTOR_1_CHA;
    motor_param[PHY_MOTOR_1].TIM_ChannelB = MOTOR_1_CHB;
    motor_param[PHY_MOTOR_1].MotorId = 1;

    //2
    motor_param[PHY_MOTOR_2].pEncoderTim = &MOTOR_2_FB;
    motor_param[PHY_MOTOR_2].pPwmTimA = &MOTOR_2_PWMA;
    motor_param[PHY_MOTOR_2].pPwmTimB = &MOTOR_2_PWMB;
    motor_param[PHY_MOTOR_2].TIM_ChannelA = MOTOR_2_CHA;
    motor_param[PHY_MOTOR_2].TIM_ChannelB = MOTOR_2_CHB;
    motor_param[PHY_MOTOR_2].MotorId = 2;

    //3
    motor_param[PHY_MOTOR_3].pEncoderTim = &MOTOR_3_FB;
    motor_param[PHY_MOTOR_3].pPwmTimA = &MOTOR_3_PWMA;
    motor_param[PHY_MOTOR_3].pPwmTimB = &MOTOR_3_PWMB;
    motor_param[PHY_MOTOR_3].TIM_ChannelA = MOTOR_3_CHA;
    motor_param[PHY_MOTOR_3].TIM_ChannelB = MOTOR_3_CHB;
    motor_param[PHY_MOTOR_3].MotorId = 3;

    //4
    motor_param[PHY_MOTOR_4].pEncoderTim = &MOTOR_4_FB;
    motor_param[PHY_MOTOR_4].pPwmTimA = &MOTOR_4_PWMA;
    motor_param[PHY_MOTOR_4].pPwmTimB = &MOTOR_4_PWMB;
    motor_param[PHY_MOTOR_4].TIM_ChannelA = MOTOR_4_CHA;
    motor_param[PHY_MOTOR_4].TIM_ChannelB = MOTOR_4_CHB;
    motor_param[PHY_MOTOR_4].MotorId = 4;


    for(uint8_t i=0; i<PHY_MOTOR_MAX; i++)
    {
        __HAL_TIM_SET_COMPARE(motor_param[i].pPwmTimA, motor_param[i].TIM_ChannelA, 0);
        __HAL_TIM_SET_COMPARE(motor_param[i].pPwmTimB, motor_param[i].TIM_ChannelB, 0);
    }

    for(uint8_t i=0; i<PHY_MOTOR_MAX; i++)
    {
        HAL_TIM_PWM_Start(motor_param[i].pPwmTimA, motor_param[i].TIM_ChannelA);
        HAL_TIM_PWM_Start(motor_param[i].pPwmTimB, motor_param[i].TIM_ChannelB);
    }

    for(uint8_t i=0; i<PHY_MOTOR_MAX; i++)
    {
        HAL_TIM_Encoder_Start(motor_param[i].pEncoderTim, TIM_CHANNEL_ALL);
    }
}


/**
* @brief 获取电机实际速度
* @param motor：电机对象 pass_tick：采样的时间计数
* @retval 电机的实际速度
*/
static float motor_get_speed(motor_cfg_t* motor, float pass_tick)
{
	float speed = APP_F_NULL;
    float plus = (short)(__HAL_TIM_GET_COUNTER(motor->pEncoderTim));
    __HAL_TIM_SET_COUNTER(motor->pEncoderTim, 0);
    speed = (plus*PULSE2METER)/pass_tick;
    //printf("id:%d plus:%f, tim:%f\r\n",Motor->MotorId, plus, pass_tick);
    if((motor->MotorId == 2)||(motor->MotorId == 4))
    {
    	return speed*-1;
    }
    else
    {
    	return speed;
    }
}


/**
* @brief 设置电机pwm输出
* @param motor_id：(电机编号1~4) PwmDuty：占空比
* @retval
*/
static inline void motor_set_pwm(uint8_t motor_id, float PwmDuty)
{
    float fDuty = 0.0;

    fDuty = (PwmDuty*-1)/TIM_ARR;

    if(fDuty > 1)
    {
        fDuty = 1;
    }
    else if(fDuty < -1)
    {
        fDuty = -1;
    }

    switch (motor_id)
    {
		case PHY_MOTOR_1:  //安装反向
			if(fDuty < 0){
				HAL_GPIO_WritePin(MOTOR_1_POA, MOTOR_1_IOA, GPIO_PIN_RESET);
				HAL_GPIO_WritePin(MOTOR_1_POB, MOTOR_1_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, (uint16_t)(-fDuty*TIM_ARR));
			}
			else if(fDuty > 0)
			{
				HAL_GPIO_WritePin(MOTOR_1_POA, MOTOR_1_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_1_POB, MOTOR_1_IOB, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, (uint16_t)(fDuty*TIM_ARR));
			}
			else
			{
				HAL_GPIO_WritePin(MOTOR_1_POA, MOTOR_1_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_1_POB, MOTOR_1_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, 0);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, 0);
			}
			break;

		case PHY_MOTOR_2: //安装反向
			if(fDuty < 0){
				HAL_GPIO_WritePin(MOTOR_2_POA, MOTOR_2_IOA, GPIO_PIN_RESET);
				HAL_GPIO_WritePin(MOTOR_2_POB, MOTOR_2_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, (uint16_t)(-fDuty*TIM_ARR));
			}
			else if(fDuty > 0)
			{
				HAL_GPIO_WritePin(MOTOR_2_POA, MOTOR_2_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_2_POB, MOTOR_2_IOB, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, (uint16_t)(fDuty*TIM_ARR));
			}
			else
			{
				HAL_GPIO_WritePin(MOTOR_2_POA, MOTOR_2_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_2_POB, MOTOR_2_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, 0);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, 0);
			}
			break;
		case PHY_MOTOR_3:
			if(fDuty < 0){
				HAL_GPIO_WritePin(MOTOR_3_POA, MOTOR_3_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_3_POB, MOTOR_3_IOB, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, (uint16_t)(-fDuty*TIM_ARR));
			}
			else if(fDuty > 0)
			{
				HAL_GPIO_WritePin(MOTOR_3_POA, MOTOR_3_IOA, GPIO_PIN_RESET);
				HAL_GPIO_WritePin(MOTOR_3_POB, MOTOR_3_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, (uint16_t)(fDuty*TIM_ARR));
			}
			else
			{
				HAL_GPIO_WritePin(MOTOR_3_POA, MOTOR_3_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_3_POB, MOTOR_3_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, 0);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, 0);
			}
			break;
		case PHY_MOTOR_4:
			if(fDuty < 0){
				HAL_GPIO_WritePin(MOTOR_4_POA, MOTOR_4_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_4_POB, MOTOR_4_IOB, GPIO_PIN_RESET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, (uint16_t)(-fDuty*TIM_ARR));
			}
			else if(fDuty > 0)
			{
				HAL_GPIO_WritePin(MOTOR_4_POA, MOTOR_4_IOA, GPIO_PIN_RESET);
				HAL_GPIO_WritePin(MOTOR_4_POB, MOTOR_1_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, (uint16_t)(fDuty*TIM_ARR));
			}
			else
			{
				HAL_GPIO_WritePin(MOTOR_4_POA, MOTOR_4_IOA, GPIO_PIN_SET);
				HAL_GPIO_WritePin(MOTOR_4_POB, MOTOR_1_IOB, GPIO_PIN_SET);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimB, motor_param[motor_id].TIM_ChannelB, 0);
				__HAL_TIM_SET_COMPARE(motor_param[motor_id].pPwmTimA, motor_param[motor_id].TIM_ChannelA, 0);
			}
			break;
		default:
			break;
    }
}


/**
* @brief 设置电机期盼速度
* @param motor_id：(电机编号1~4) SpeedTar：电机的期盼速度
* @retval 无
*/
void motor_set_speed(uint8_t motor_id, float SpeedTar)
{
	motor_param[motor_id].PID.SetTarget = SpeedTar;
}


/**
* @brief 获取时间计数
* @param 无
* @retval 时间计数(秒)
*/
static inline float mobile_get_tick(void)
{
	float tmp = mobile_tick;
    return (tmp/1000);
}


/**
* @brief 清除时间计数
* @prime 无
* @retval 无
*/
static inline void mobile_clr_tick(void)
{
	mobile_tick = APP_NULL;
}


/**
* @brief 设置底盘当前期盼速度
* @param m1：电机一速度值 m2：电机二速度值 m3：电机三速度值 m4：电机四速度值
* @retval 无
*/
static void mobile_set_speed(float m1, float m2, float m3, float m4)
{
	xbot.mobile.motor[PHY_MOTOR_1].fPreSPD = target_limit(m1, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_2].fPreSPD = target_limit(m2, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_3].fPreSPD = target_limit(m3, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_4].fPreSPD = target_limit(m4, SPEED_MIN, SPEED_MAX);
	motor_set_speed(PHY_MOTOR_1, xbot.mobile.motor[PHY_MOTOR_1].fPreSPD);
	motor_set_speed(PHY_MOTOR_2, xbot.mobile.motor[PHY_MOTOR_2].fPreSPD);
	motor_set_speed(PHY_MOTOR_3, xbot.mobile.motor[PHY_MOTOR_3].fPreSPD);
	motor_set_speed(PHY_MOTOR_4, xbot.mobile.motor[PHY_MOTOR_4].fPreSPD);
}


/**
* @brief 底盘定时计数器
* @param 无
* @retval 无
*/
void mobile_timer(void)
{
    mobile_tick += 1;

}


/**
* @brief 查询底盘状态
* @param 无
* @retval 1：底盘空闲  0：底盘正在运动
*/
int mobile_is_ready(void)
{
	if(xbot.mobile.state == SYS_FLAG_IDLE)
	{
		return 1;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
void mobile_ik(float vx,float vy,float wz)
{
	mecanum_ik(vx,vy,wz,
			&xbot.mobile.motor[PHY_MOTOR_1].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_2].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_3].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_4].fPreSPD
			);
	mobile_set_speed(
			xbot.mobile.motor[PHY_MOTOR_1].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_2].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_3].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_4].fPreSPD
			);
//	printf("ik:%f %f %f\r\n",vx,vy,wz);
//	printf("pre:%f %f %f %f\r\n",
//			xbot.mobile.motor[PHY_MOTOR_1].fPreSPD,
//			xbot.mobile.motor[PHY_MOTOR_2].fPreSPD,
//			xbot.mobile.motor[PHY_MOTOR_3].fPreSPD,
//			xbot.mobile.motor[PHY_MOTOR_4].fPreSPD);
}


/**
* @brief
* @param
* @retval
*/
void mobile_fk(float tim)
{
	float vx,vy,wz;
	mecanum_fk(xbot.mobile.motor[PHY_MOTOR_1].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_2].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_3].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_4].fActSPD,
			&vx,
			&vy,
			&wz
			);
// 时间积分
	xbot.mobile.pose.position.X += vx*tim;
	xbot.mobile.pose.position.Y	+= vy*tim;

// 欧拉积分
//	xbot.mobile.pose.position.X += vx*cos(xbot.mobile.pose.angular.Z)*tim;
//	xbot.mobile.pose.position.Y += vy*sin(xbot.mobile.pose.angular.Z)*tim;

// 二阶Runge-Kutta积分
//	xbot.mobile.pose.position.X += vx*cos(xbot.mobile.pose.angular.Z+xbot.mobile.pose.angular.Z/2)*tim;
//	xbot.mobile.pose.position.Y += vy*sin(xbot.mobile.pose.angular.Z+xbot.mobile.pose.angular.Z/2)*tim;


}


/**
* @brief
* @param
* @retval
*/
int mobile_move_x(float x)
{
	static int step = 0;
	switch(step)
	{
		case 0:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				if(x >= 0)
				{
					mobile_run_dir = 1;
				}
				else if(x < 0)
				{
					mobile_run_dir = 0;
				}
				mobile_run_axis = MOBILE_AXIS_X;
				mobile_target.position.X = x + xbot.mobile.pose.position.X;
				xbot.mobile.state = SYS_FLAG_PLAY;
				step = 1;
			}
			break;
		case 1:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;

	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int mobile_move_y(float y)
{
	static int step = 0;
	switch(step)
	{
		case 0:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				if(y >= 0)
				{
					mobile_run_dir = 1;
				}
				else if(y < 0)
				{
					mobile_run_dir = 0;
				}
				mobile_run_axis = MOBILE_AXIS_Y;
				mobile_target.position.Y = y + xbot.mobile.pose.position.Y;
				xbot.mobile.state = SYS_FLAG_PLAY;
				step = 1;
			}
			break;
		case 1:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;
		default:
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int mobile_rotation_z(float rad)
{
	static int step = 0;
	float tmp = 0;
	switch(step)
	{
		case 0:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				if(rad < xbot.mobile.pose.angular.Z)
				{
					mobile_run_dir = 0;
					tmp = 0.0736;
				}
				else if(rad >= xbot.mobile.pose.angular.Z)
				{
					mobile_run_dir = 1;
					tmp = -0.0736;
				}
				mobile_run_axis = MOBILE_AXIS_R;
				mobile_target.angular.Z = rad+tmp;					// 0.0742 为偏差补偿
				xbot.mobile.state = SYS_FLAG_PLAY;
				step = 1;
			}
			break;
		case 1:
			if(xbot.mobile.state == SYS_FLAG_IDLE)
			{
				step = 0;
				return 1;
			}
			break;
	}
	return 0;
}


/**
* @brief 底盘初始化函数
* @param 无
* @retval 无
*/
void mobile_init(void)
{
	xbot.mobile.state = SYS_FLAG_STARTUP;											// 底盘初始状态
	motor_init();																	// 电机参数及接口初始化
	mobile_target.position.X = APP_F_NULL;
	mobile_target.position.Y = APP_F_NULL;
	mobile_target.position.Z = APP_F_NULL;
	mobile_target.angular.X = APP_F_NULL;
	mobile_target.angular.Y = APP_F_NULL;
	mobile_target.angular.Z = APP_F_NULL;
	xbot.mobile.state = SYS_FLAG_IDLE;												// 底盘进入空闲状态
}


/**
* @brief 底盘反初始化函数
* @param 无
* @retval 无
*/
void mobile_deinit(void)
{
	mobile_set_speed(APP_F_NULL, APP_F_NULL, APP_F_NULL, APP_F_NULL);
	motor_set_speed(PHY_MOTOR_1, xbot.mobile.motor[PHY_MOTOR_1].fPreSPD);
	motor_set_speed(PHY_MOTOR_2, xbot.mobile.motor[PHY_MOTOR_2].fPreSPD);
	motor_set_speed(PHY_MOTOR_3, xbot.mobile.motor[PHY_MOTOR_3].fPreSPD);
	motor_set_speed(PHY_MOTOR_4, xbot.mobile.motor[PHY_MOTOR_4].fPreSPD);
	xbot.mobile.state = SYS_FLAG_SHUTDOWN;
}


/**
* @brief
* @param
* @retval
*/
void mobile_polling(void)
{
	static float _pwm[PHY_MOTOR_MAX]={APP_F_NULL, APP_F_NULL, APP_F_NULL, APP_F_NULL};
	float timepass = mobile_get_tick();
	mobile_clr_tick();
	xbot.mobile.motor[PHY_MOTOR_1].fActSPD = motor_get_speed(&motor_param[PHY_MOTOR_1], timepass);
	xbot.mobile.motor[PHY_MOTOR_2].fActSPD = motor_get_speed(&motor_param[PHY_MOTOR_2], timepass);
	xbot.mobile.motor[PHY_MOTOR_3].fActSPD = motor_get_speed(&motor_param[PHY_MOTOR_3], timepass);
	xbot.mobile.motor[PHY_MOTOR_4].fActSPD = motor_get_speed(&motor_param[PHY_MOTOR_4], timepass);
	mobile_fk(timepass);

//	static uint16_t ss = 0;

	switch(xbot.mobile.state)
	{
		case SYS_FLAG_PLAY:
			switch(mobile_run_axis)
			{
				case MOBILE_AXIS_X:
					if(mobile_run_dir)
					{
						if(xbot.mobile.pose.position.X >= mobile_target.position.X)
						{
							if((mobile_run_speed - MOBILE_SPD_DELTA) > APP_F_NULL)
							{
								// 平滑减速过程
								mobile_run_speed -= MOBILE_SPD_DELTA;
							}
							else
							{
//								ss = 0;
								mobile_run_speed = APP_F_NULL;
								xbot.mobile.state = SYS_FLAG_IDLE;
							}
						}
						else
						{
							mobile_run_speed = MOBILE_X_AVG_SPD;
						}
					}
					else
					{
						if(xbot.mobile.pose.position.X <= mobile_target.position.X)
						{
							if((mobile_run_speed + MOBILE_SPD_DELTA) > APP_F_NULL)
							{
								// 平滑减速过程
								mobile_run_speed += MOBILE_SPD_DELTA;
							}
							else
							{
								mobile_run_speed = APP_F_NULL;
								xbot.mobile.state = SYS_FLAG_IDLE;
							}
						}
						else
						{
							mobile_run_speed = -MOBILE_X_AVG_SPD;
						}
					}
					if(xbot.mobile.pose.angular.Z > 0)
					{
						mobile_run_angle = -MOBILE_R_AVG_SPD;
					}
					else if(xbot.mobile.pose.angular.Z < 0)
					{
						mobile_run_angle = MOBILE_R_AVG_SPD;
					}
					else
					{
						mobile_run_angle = 0.0f;
					}
					mobile_ik(mobile_run_speed, APP_F_NULL, mobile_run_angle);
					break;
				case MOBILE_AXIS_Y:
					if(mobile_run_dir)
					{
						if(xbot.mobile.pose.position.Y >= mobile_target.position.Y)
						{
							if((mobile_run_speed - MOBILE_SPD_DELTA) > APP_F_NULL)
							{
								// 平滑减速过程
								mobile_run_speed -= MOBILE_SPD_DELTA;
							}
							else
							{
								mobile_run_speed = APP_F_NULL;
								xbot.mobile.state = SYS_FLAG_IDLE;
							}
						}
						else
						{
							mobile_run_speed = MOBILE_Y_AVG_SPD;
						}
					}
					else
					{
						if(xbot.mobile.pose.position.Y <= mobile_target.position.Y)
						{
							if((mobile_run_speed + MOBILE_SPD_DELTA) > APP_F_NULL)
							{
								// 平滑减速过程
								mobile_run_speed += MOBILE_SPD_DELTA;
							}
							else
							{
								mobile_run_speed = APP_F_NULL;
								xbot.mobile.state = SYS_FLAG_IDLE;
							}
						}
						else
						{
							mobile_run_speed = -MOBILE_Y_AVG_SPD;
						}
					}
					if(xbot.mobile.pose.angular.Z > 0)
					{
						mobile_run_angle = -MOBILE_R_AVG_SPD;
					}
					else if(xbot.mobile.pose.angular.Z < 0)
					{
						mobile_run_angle = MOBILE_R_AVG_SPD;
					}
					else
					{
						mobile_run_angle = 0.0f;
					}
					mobile_ik(APP_F_NULL, mobile_run_speed, mobile_run_angle);
					break;
				case MOBILE_AXIS_R:
					if(mobile_run_dir)
					{
						if(xbot.mobile.pose.angular.Z >= mobile_target.angular.Z)
						{
							mobile_set_speed(APP_F_NULL, APP_F_NULL, APP_F_NULL, APP_F_NULL);
							xbot.mobile.state = SYS_FLAG_IDLE;
						}
						else
						{
							mobile_set_speed(-MOBILE_DEF_SPD, MOBILE_DEF_SPD, -MOBILE_DEF_SPD, MOBILE_DEF_SPD);
						}
					}
					else
					{
						if(xbot.mobile.pose.angular.Z <= mobile_target.angular.Z)
						{
							mobile_set_speed(APP_F_NULL, APP_F_NULL, APP_F_NULL, APP_F_NULL);
							xbot.mobile.state = SYS_FLAG_IDLE;
						}
						else
						{
							mobile_set_speed(MOBILE_DEF_SPD, -MOBILE_DEF_SPD, MOBILE_DEF_SPD, -MOBILE_DEF_SPD);
						}
					}
					break;
				default:
					mobile_ik(0, 0, 0);
					break;
			}

//// TEST PID (matlab:pid_chart.m)
//			printf("%f %f %f %f %f\r\n",
//					xbot.mobile.motor[PHY_MOTOR_1].fActSPD,
//					xbot.mobile.motor[PHY_MOTOR_2].fActSPD,
//					xbot.mobile.motor[PHY_MOTOR_3].fActSPD,
//					xbot.mobile.motor[PHY_MOTOR_4].fActSPD,
//					timepass);
//// TEST PID

//// TEST TOR (matlab:motor_chart.m)
//			ss++;
//			_pwm[PHY_MOTOR_1] = ss+315;
//			_pwm[PHY_MOTOR_2] = ss+389;
//			_pwm[PHY_MOTOR_3] = ss+375;
//			_pwm[PHY_MOTOR_4] = ss+377;
//						printf("%f %f %f %f %d\r\n",
//								xbot.mobile.motor[PHY_MOTOR_1].fActSPD,
//								xbot.mobile.motor[PHY_MOTOR_2].fActSPD,
//								xbot.mobile.motor[PHY_MOTOR_3].fActSPD,
//								xbot.mobile.motor[PHY_MOTOR_4].fActSPD,
//								ss);
//// TEST TOR

			_pwm[PHY_MOTOR_1] = pid_update(&motor_param[PHY_MOTOR_1].PID, xbot.mobile.motor[PHY_MOTOR_1].fActSPD);
			// 电机一增加前馈量
			if(_pwm[PHY_MOTOR_1] > 0)
			{
				_pwm[PHY_MOTOR_1] += MOTOR_1_FF;
			}
			else if(_pwm[PHY_MOTOR_1] < 0)
			{
				_pwm[PHY_MOTOR_1] -= MOTOR_1_FF;
			}
			else
			{
				_pwm[PHY_MOTOR_1] = APP_F_NULL;
			}

			_pwm[PHY_MOTOR_2] = pid_update(&motor_param[PHY_MOTOR_2].PID, xbot.mobile.motor[PHY_MOTOR_2].fActSPD);
			// 电机二增加前馈量
			if(_pwm[PHY_MOTOR_2] > 0)
			{
				_pwm[PHY_MOTOR_2] += MOTOR_2_FF;
			}
			else if(_pwm[PHY_MOTOR_2] < 0)
			{
				_pwm[PHY_MOTOR_2] -= MOTOR_2_FF;
			}
			else
			{
				_pwm[PHY_MOTOR_2] = APP_F_NULL;
			}

			_pwm[PHY_MOTOR_3] = pid_update(&motor_param[PHY_MOTOR_3].PID, xbot.mobile.motor[PHY_MOTOR_3].fActSPD);
			// 电机三增加前馈量
			if(_pwm[PHY_MOTOR_3] > 0)
			{
				_pwm[PHY_MOTOR_3] += MOTOR_3_FF;
			}
			else if(_pwm[PHY_MOTOR_3] < 0)
			{
				_pwm[PHY_MOTOR_3] -= MOTOR_3_FF;
			}
			else
			{
				_pwm[PHY_MOTOR_3] = APP_F_NULL;
			}

			_pwm[PHY_MOTOR_4] = pid_update(&motor_param[PHY_MOTOR_4].PID, xbot.mobile.motor[PHY_MOTOR_4].fActSPD);
			// 电机四增加前馈量
			if(_pwm[PHY_MOTOR_4] > 0)
			{
				_pwm[PHY_MOTOR_4] += MOTOR_4_FF;
			}
			else if(_pwm[PHY_MOTOR_4] < 0)
			{
				_pwm[PHY_MOTOR_4] -= MOTOR_4_FF;
			}
			else
			{
				_pwm[PHY_MOTOR_4] = APP_F_NULL;
			}

			break;
		case SYS_FLAG_IDLE:
			pid_init(&motor_param[PHY_MOTOR_1].PID, MOTOR_1_KP, MOTOR_1_KI, APP_F_NULL);
			pid_init(&motor_param[PHY_MOTOR_2].PID, MOTOR_2_KP, MOTOR_2_KI, APP_F_NULL);
			pid_init(&motor_param[PHY_MOTOR_3].PID, MOTOR_3_KP, MOTOR_3_KI, APP_F_NULL);
			pid_init(&motor_param[PHY_MOTOR_4].PID, MOTOR_4_KP, MOTOR_4_KI, APP_F_NULL);
			_pwm[PHY_MOTOR_1] = APP_F_NULL;
			_pwm[PHY_MOTOR_2] = APP_F_NULL;
			_pwm[PHY_MOTOR_3] = APP_F_NULL;
			_pwm[PHY_MOTOR_4] = APP_F_NULL;
			break;
	}

	motor_set_pwm(PHY_MOTOR_1, _pwm[PHY_MOTOR_1]);
	motor_set_pwm(PHY_MOTOR_2, _pwm[PHY_MOTOR_2]);
	motor_set_pwm(PHY_MOTOR_3, _pwm[PHY_MOTOR_3]);
	motor_set_pwm(PHY_MOTOR_4, _pwm[PHY_MOTOR_4]);

// 测试电机
//		motor_set_pwm(PHY_MOTOR_1, 500);
//		motor_set_pwm(PHY_MOTOR_2, 500);
//		motor_set_pwm(PHY_MOTOR_3, 500);
//		motor_set_pwm(PHY_MOTOR_4, 500);

}
#endif


#if MOTOR_STEP42

static uint32_t mobile_tick = 0;

static motor_cfg_t motor_cfg[PHY_MOTOR_MAX];

static uint8_t mobile_run_axis = 0;
static int8_t mobile_run_dir = 0;
static float mobile_run_speed;
static float mobile_run_angle;

static pose_t mobile_target;

static uint8_t run_step = 0;


/**
* @brief
* @param
* @retval
*/
static void inline motor_set_speed(uint8_t phy_dev, float speed)
{
	uint16_t rad = (fabs(speed)/WHEEL_RADIUS)*10;

	motor_cfg[phy_dev].can_msg[0] = 0x01;				// 1 为速度控制模式，2 为位置控制模式，3 为力矩控制模式、4 为单圈绝对角度控制模式
	switch(phy_dev)
	{
		case PHY_MOTOR_1:
		case PHY_MOTOR_3:
			if(speed>=0)
			{
				motor_cfg[phy_dev].can_msg[1] = 0x01;								// 0 为逆时针转动，1 为顺时针转动
			}
			else
			{
				motor_cfg[phy_dev].can_msg[1] = 0x00;
			}
			break;
		case PHY_MOTOR_2:
		case PHY_MOTOR_4:
			if(speed>=0)
			{
				motor_cfg[phy_dev].can_msg[1] = 0x00;								// 0 为逆时针转动，1 为顺时针转动
			}
			else
			{
				motor_cfg[phy_dev].can_msg[1] = 0x01;
			}
			break;
	}

	motor_cfg[phy_dev].can_msg[2] = 0x20;				// 进电机细分值。例如：0x20=32，就是 32 细分，支持 2、4、8、16、32 细分。（尽量不要在 2、4 细分值下使用位置控制模式和绝对角度控制模式，会影响精度）
/*
 * 速度控制模式下为 0；位置控制模式下的角度数据，单位为角度，放大十倍传输，分别为高八位和低八位。例如:0x27、0x10=0x2710=10000=1000 度
 * 力矩控制模式下的电流大小数据，单位为毫安，分别为高八位和低八位。例如:0x03、0xe8 =0x03e8=1000mA
 * 绝对角度控制模式下的目标角度，放大十倍发送，分别为高八位和低八位。例如:0x03、0xe8=0x03e8=1000/10.0=100 度
 */
	motor_cfg[phy_dev].can_msg[3] = 0x00;
	motor_cfg[phy_dev].can_msg[4] = 0x00;
 /*
  * 转速数据，单位为弧度/s，放大十倍传输，分别为高八位和低八位。例如:0x00、0x64 =0x0064=100=10Rad/s
  */

	motor_cfg[phy_dev].can_msg[5] = (uint8_t)(rad>>8);
    motor_cfg[phy_dev].can_msg[6] = (uint8_t)rad;


    motor_cfg[phy_dev].state = APP_TRUE;

}


/**
* @brief
* @param
* @retval
*/
static inline void mobile_set_speed(float m1, float m2, float m3, float m4)
{
	xbot.mobile.motor[PHY_MOTOR_1].fPreSPD = target_limit(m1, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_2].fPreSPD = target_limit(m2, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_3].fPreSPD = target_limit(m3, SPEED_MIN, SPEED_MAX);
	xbot.mobile.motor[PHY_MOTOR_4].fPreSPD = target_limit(m4, SPEED_MIN, SPEED_MAX);
	motor_set_speed(PHY_MOTOR_1, xbot.mobile.motor[PHY_MOTOR_1].fPreSPD);
	motor_set_speed(PHY_MOTOR_2, xbot.mobile.motor[PHY_MOTOR_2].fPreSPD);
	motor_set_speed(PHY_MOTOR_3, xbot.mobile.motor[PHY_MOTOR_3].fPreSPD);
	motor_set_speed(PHY_MOTOR_4, xbot.mobile.motor[PHY_MOTOR_4].fPreSPD);
}


/**
* @brief
* @param
* @retval
*/
int mobile_is_ready(void)
{
	if(xbot.mobile.state == SYS_FLAG_IDLE)
	{
		return 1;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
static inline void mobile_ik(float vx,float vy,float wz)
{
	mecanum_ik(vx,vy,wz,
			&xbot.mobile.motor[PHY_MOTOR_1].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_2].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_3].fPreSPD,
			&xbot.mobile.motor[PHY_MOTOR_4].fPreSPD
			);
	mobile_set_speed(
			xbot.mobile.motor[PHY_MOTOR_1].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_2].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_3].fPreSPD,
			xbot.mobile.motor[PHY_MOTOR_4].fPreSPD
			);
}


/**
* @brief
* @param
* @retval
*/
static inline void mobile_fk(float tim)
{
	float vx,vy,wz;

	mecanum_fk(xbot.mobile.motor[PHY_MOTOR_1].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_2].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_3].fActSPD,
			xbot.mobile.motor[PHY_MOTOR_4].fActSPD,
			&vx,
			&vy,
			&wz
			);

// 时间积分
	xbot.mobile.pose.position.X += vx*tim;
	xbot.mobile.pose.position.Y	+= vy*tim;

// 欧拉积分
//	xbot.mobile.pose.position.X += vx*cos(xbot.mobile.pose.angular.Z)*tim;
//	xbot.mobile.pose.position.Y += vy*sin(xbot.mobile.pose.angular.Z)*tim;

// 二阶Runge-Kutta积分
//	xbot.mobile.pose.position.X += vx*cos(xbot.mobile.pose.angular.Z+xbot.mobile.pose.angular.Z/2)*tim;
//	xbot.mobile.pose.position.Y += vy*sin(xbot.mobile.pose.angular.Z+xbot.mobile.pose.angular.Z/2)*tim;

}


/**
* @brief
* @param
* @retval
*/
static inline float mobile_get_tick(void)
{
	float tmp = mobile_tick;
    return (tmp/1000);
}


/**
* @brief
* @prime
* @retval
*/
static inline void mobile_clr_tick(void)
{
	mobile_tick = APP_NULL;
}


/**
* @brief
* @param
* @retval
*/
void mobile_timer(void)
{
	mobile_tick += 1;
}


/**
* @brief
* @param
* @retval
*/
void mobile_move_x(float x)
{
	if(xbot.mobile.state == SYS_FLAG_IDLE)
	{
//		if(x > 0)
//		{
//			mobile_run_dir = 1;
//		}
//		else if(x < 0)
//		{
//			mobile_run_dir = 0;
//		}
//		else
//		{
//			return;
//		}
		mobile_run_axis = MOBILE_AXIS_X;


		mobile_ik(0.05, 0, 0);
		mobile_target.position.X = x + xbot.mobile.pose.position.X;
		xbot.mobile.state = SYS_FLAG_PLAY;
	}
}


/**
* @brief
* @param
* @retval
*/
void mobile_move_y(float y)
{
	if(xbot.mobile.state == SYS_FLAG_IDLE)
	{
		mobile_run_axis = MOBILE_AXIS_Y;
		mobile_target.position.Y = y + xbot.mobile.pose.position.Y;
		xbot.mobile.state = SYS_FLAG_PLAY;
	}
}


/**
* @brief
* @param
* @retval
*/
void mobile_rotation_z(float rad)
{
	if(xbot.mobile.state == SYS_FLAG_IDLE)
	{
		if(rad < xbot.mobile.pose.angular.Z)
		{
			mobile_run_dir = 0;
		}
		else if(rad > xbot.mobile.pose.angular.Z)
		{
			mobile_run_dir = 1;
		}
		else
		{
			return;
		}
		mobile_target.angular.Z = rad;
		xbot.mobile.state = SYS_FLAG_PLAY;
	}
}


/**
* @brief
* @param
* @retval
*/
void mobile_init(void)
{
	xbot.mobile.state = SYS_FLAG_STARTUP;

	can_init();

	motor_cfg[PHY_MOTOR_1].id = 0x01;
	motor_cfg[PHY_MOTOR_1].state = APP_FALSE;
	memset(motor_cfg[PHY_MOTOR_1].can_msg, 0, 7);

	motor_cfg[PHY_MOTOR_2].id = 0x02;
	motor_cfg[PHY_MOTOR_2].state = APP_FALSE;
	memset(motor_cfg[PHY_MOTOR_2].can_msg, 0, 7);

	motor_cfg[PHY_MOTOR_3].id = 0x03;
	motor_cfg[PHY_MOTOR_3].state = APP_FALSE;
	memset(motor_cfg[PHY_MOTOR_3].can_msg, 0, 7);

	motor_cfg[PHY_MOTOR_4].id = 0x04;
	motor_cfg[PHY_MOTOR_4].state = APP_FALSE;
	memset(motor_cfg[PHY_MOTOR_4].can_msg, 0, 7);


	xbot.mobile.state = SYS_FLAG_IDLE;

	mobile_set_speed(0.0f, 0.0f, 0.0f, 0.0f);

}


/**
* @brief
* @param
* @retval
*/
void mobile_deinit(void)
{
	motor_set_speed(PHY_MOTOR_1, APP_F_NULL);
	motor_set_speed(PHY_MOTOR_2, APP_F_NULL);
	motor_set_speed(PHY_MOTOR_3, APP_F_NULL);
	motor_set_speed(PHY_MOTOR_4, APP_F_NULL);
	xbot.mobile.state = SYS_FLAG_SHUTDOWN;
}


/**
* @brief
* @param
* @retval
*/
void mobile_polling(void)
{
	float timepass = mobile_get_tick();
	uint8_t queue_msg[8] = {0, 0, 0, 0, 0, 0, 0, 0};
	mobile_clr_tick();
	mobile_fk(timepass);
	switch(xbot.mobile.state)
	{
		case SYS_FLAG_PLAY:
			switch(mobile_run_axis)
			{
				case MOBILE_AXIS_X:
					break;
				case MOBILE_AXIS_Y:
					if(xbot.mobile.pose.angular.Z > 0)
					{
						mobile_run_angle = -MOBILE_R_AVG_SPD;
					}
					else if(xbot.mobile.pose.angular.Z < 0)
					{
						mobile_run_angle = MOBILE_R_AVG_SPD;
					}
					else
					{
						mobile_run_angle = 0.0f;
					}

					if(mobile_target.position.Y>=0)
					{
						if(xbot.mobile.pose.position.Y >= mobile_target.position.Y)
						{
							mobile_run_speed = APP_F_NULL;
							xbot.mobile.state = SYS_FLAG_IDLE;
						}
						else
						{
							mobile_run_speed = MOBILE_DEF_SPD;
						}
					}
					else
					{
						if(xbot.mobile.pose.position.Y <= mobile_target.position.Y)
						{
							mobile_run_speed = APP_F_NULL;
							xbot.mobile.state = SYS_FLAG_IDLE;
						}
						else
						{
							mobile_run_speed = MOBILE_DEF_SPD;
						}
					}
					mobile_ik(APP_F_NULL, -MOBILE_DEF_SPD, mobile_run_angle);
					break;
				case MOBILE_AXIS_R:
					break;
				default:
					break;
			}
			break;
		case SYS_FLAG_IDLE:
			mobile_set_speed(0.0f, 0.0f, 0.0f, 0.0f);
			break;
		default:
			break;
	}
	switch(run_step)
	{
		case 0:
			can_set_msg(motor_cfg[PHY_MOTOR_1].id, motor_cfg[PHY_MOTOR_1].can_msg, 7);
			run_step = 1;
			break;
		case 1:
			can_set_msg(motor_cfg[PHY_MOTOR_2].id, motor_cfg[PHY_MOTOR_2].can_msg, 7);
			run_step = 2;
			break;
		case 2:
			can_set_msg(motor_cfg[PHY_MOTOR_3].id, motor_cfg[PHY_MOTOR_3].can_msg, 7);
			run_step = 3;
			break;
		case 3:
			can_set_msg(motor_cfg[PHY_MOTOR_4].id, motor_cfg[PHY_MOTOR_4].can_msg, 7);
			run_step = 4;
			break;
		case 4:
			can_set_msg(motor_cfg[PHY_MOTOR_1].id, queue_msg, 7);
			run_step = 5;
			break;
		case 5:
			can_set_msg(motor_cfg[PHY_MOTOR_2].id, queue_msg, 7);
			run_step = 6;
			break;
		case 6:
			can_set_msg(motor_cfg[PHY_MOTOR_3].id, queue_msg, 7);
			run_step = 7;
			break;
		case 7:
			can_set_msg(motor_cfg[PHY_MOTOR_4].id, queue_msg, 7);
			run_step = 0;
			break;
		default:
			run_step = 0;
			break;
	}
}

#endif
